import pygame
import socket
import time
ip = '192.168.137.9'
port = 24
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

BLACK    = (   0,   0,   0)
WHITE    = ( 255, 255, 255)

def verify(data1, *data):
    check = data1
    for var in data:
        check = check + var
    check = (check+1105)%120
    return check

def sendMsg(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,Light,LED):
    check = verify(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,Light,LED)
    print(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,Light,LED,check)
    s.sendto(bytes(f"ST{chr(UD)}{chr(SpeedUD)}{chr(FB)}{chr(SpeedFB)}{chr(LR)}{chr(SpeedLR)}"\
        f"{chr(SH)}{chr(SpeedSH)}{chr(POS)}{chr(PIT)}{chr(SpeedPIT)}{chr(ROL)}{chr(SpeedROL)}{chr(Light)}{chr(LED)}{chr(check)}ED".encode("utf-8")), (ip, port)) #0到17共18位

class TextPrint:
    def __init__(self):
        self.reset()
        self.font = pygame.font.Font(None, 20)
 
    def print(self, screen, textString):
        textBitmap = self.font.render(textString, True, BLACK)
        screen.blit(textBitmap, [self.x, self.y])
        self.y += self.line_height
        
    def reset(self):
        self.x = 10
        self.y = 10
        self.line_height = 15
        
    def indent(self):
        self.x += 10
        
    def unindent(self):
        self.x -= 10
     
pygame.init()

size = [500, 700]
screen = pygame.display.set_mode(size)
pygame.display.set_caption("My Game")
 
done = False
clock = pygame.time.Clock()
pygame.joystick.init()
textPrint = TextPrint()

#摇杆初始化矫正
# origin = [0,0,0,0,0,0,0,0]
# joystick_count = 1
# joystick = pygame.joystick.Joystick(joystick_count)
# joystick.init()
# axes = joystick.get_numaxes()

# for i in range( axes ):
#     axis = joystick.get_axis(i)
#     origin[i] = joystick.get_axis(i)
# print(origin)
button = [0 for i in range(16)]

Light = 0
LED = 0

while done==False:
    for event in pygame.event.get(): 
        if event.type == pygame.QUIT: 
            done=True 

    screen.fill(WHITE)
    textPrint.reset()

    #joystick_count = pygame.joystick.get_count()
    joystick_count = 1
    
    textPrint.indent()

    for i in range(joystick_count):
        joystick = pygame.joystick.Joystick(i)
        joystick.init()
        axes = joystick.get_numaxes()

        MAX = 50
        FB = 0
        LR = 0
        UD = 0
        SH = 0
        SpeedFB = 0
        SpeedLR = 0
        SpeedUD = 0
        SpeedSH = 0
        update = 0
        POS = 0
        PIT = 0
        SpeedPIT = 0
        ROL = 0
        SpeedROL = 0
        for i in range( axes ):
            axis = joystick.get_axis(i)
            
            if i==4 and axis > -0.8:# LT   float
                update = 1
                UD = 1
                SpeedUD = int((axis+1)*MAX*0.5)
                textPrint.print(screen, "Axis{} Floating: {:>6.3f}".format(i,SpeedUD) )
            if i==5 and axis > -0.8:# RT   dive
                update = 1
                UD = 2
                SpeedUD = int((axis+1)*MAX*0.5)
                textPrint.print(screen, "Axis{} Diving: {:>6.3f}".format(i,SpeedUD) )
            if i==0 and axis< -0.09:#左摇杆 left
                update = 1
                LR = 1
                SpeedLR = -int(axis*MAX)
                textPrint.print(screen, "Axis{} TurningLeft: {:>6.3f}".format(i,SpeedLR) )
            if i==0 and axis > 0.09:#左摇杆 right
                update = 1
                LR = 2
                SpeedLR = int(axis*MAX)
                textPrint.print(screen, "Axis{} TurningRight: {:>6.3f}".format(i,SpeedLR) )
            if i==1 and axis< -0.09:#左摇杆 
                update = 1
                PIT = 1
                SpeedPIT = -int(axis*MAX)
                textPrint.print(screen, "Axis{} LowerHead: {:>6.3f}".format(i,SpeedPIT) )
            if i==1 and axis > 0.09:#左摇杆 
                update = 1
                PIT = 2
                SpeedPIT = int(axis*MAX)
                textPrint.print(screen, "Axis{} RiseHead: {:>6.3f}".format(i,SpeedPIT) )
            if i==3 and axis< -0.09:
                update = 1
                FB = 1
                SpeedFB = -int(axis*MAX)
                textPrint.print(screen, "Axis{} Forward: {:>6.3f}".format(i,SpeedFB) )
            if i==3 and axis > 0.09:
                update = 1
                FB = 2
                SpeedFB = int(axis*MAX)
                textPrint.print(screen, "Axis{} Back: {:>6.3f}".format(i,SpeedFB) )
            if i==2 and axis< -0.09:
                update = 1
                SH = 1
                SpeedSH = -int(axis*MAX)
                textPrint.print(screen, "Axis{} ShiftLeft: {:>6.3f}".format(i,SpeedSH) )
            if i==2 and axis > 0.09:
                update = 1
                SH = 2
                SpeedSH = int(axis*MAX)
                textPrint.print(screen, "Axis{} ShiftRight: {:>6.3f}".format(i,SpeedSH) )
        textPrint.unindent()
        
        buttons = joystick.get_numbuttons()

        textPrint.indent()
 
        for i in range( buttons ):
            button[i] = joystick.get_button( i )
            # textPrint.print(screen, "Button {:>2} value: {}".format(i,button) )
            if i==0 and button[i] ==1:
                if(hold_led == 255):
                    if(LED != 3):
                        LED = 3
                    else:
                        LED = 0
                    print("A")
                    hold_led = 0
            if i==1 and button[i] ==1:
                if(hold_led == 255):
                    if(LED != 1):
                        LED = 1
                    else:
                        LED = 0
                    print("B")
                    hold_led = 1
            if i==2 and button[i] ==1:
                if(hold_led == 255):
                    if(LED != 4):
                        LED = 4
                    else:
                        LED = 0
                    print("X")
                    hold_led = 2
            if i==3 and button[i] ==1:
                if(hold_led == 255):
                    if(LED != 2):
                        LED = 2
                    else:
                        LED = 0
                    print("Y")
                    hold_led = 3

            if(button[0]+button[1]+button[2]+button[3]==0):
                hold_led = 255
            
            if i==4 and button[i] ==1:
                POS = 1
                textPrint.print(screen, "Button {} LB Posture".format(i) )

            
            
            if i==5 and button[i] ==1:
                print("RB")
            if i==6 and button[i] ==1:
                print("BACK")
            if i==7 and button[i] ==1:
                print("START")
            if i==8 and button[i] ==1:
                print("Logitech")
            if i==9 and button[i] ==1:
                print("Left GA")
            if i==10 and button[i] ==1:
                print("Right GA")
 
        textPrint.unindent()

        hats = joystick.get_numhats()
        textPrint.print(screen, "Number of hats: {}".format(hats) )
        textPrint.indent()
 
        for i in range( hats ):
            hat = joystick.get_hat( i )
            textPrint.print(screen, "Hat {} value: {}".format(i, str(hat)) )
            if hat==(1,0) :
                print("FX right")
            if hat==(-1,0) :
                print("FX left")
            if hat==(0,1):
                if(hold == 0):
                    hold = 1
                    Light = Light + 2
                    if Light > 35:
                        Light = 34
                    if Light <0:
                        Light = 0
                print("FX up")
            if hat==(0,-1):
                if(hold == 0):
                    hold = 1
                    Light = Light - 2
                    if Light > 24:
                        Light = 24
                    if Light <0:
                        Light = 0
                print("FX down")
            if hat == (0,0):
                hold = 0
        textPrint.unindent()
        if(POS == 1):
            ROL = LR
            SpeedROL = SpeedLR
            LR = 0
            SpeedLR = 0
        else:
            PIT = 0
            SpeedPIT = 0
        sendMsg(UD,SpeedUD,FB,SpeedFB,LR,SpeedLR,SH,SpeedSH,POS,PIT,SpeedPIT,ROL,SpeedROL,Light,LED)
    pygame.display.flip()
    time.sleep(0.005)

    
pygame.quit ()

